/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-07     Hao       the first version
 */
#ifndef FOC_UTILS_H
#define FOC_UTILS_H

#include <stdint.h>

// 常量定义
#define _2_SQRT3 			1.15470053838f
#define _SQRT3 				1.73205080757f
#define _1_SQRT3 			0.57735026919f
#define _SQRT3_2 			0.86602540378f
#define _SQRT2 				1.41421356237f
#define _120_D2R 			2.09439510239f
#define _PI 				3.14159265359f
#define _PI_2 				1.57079632679f
#define _PI_3 				1.0471975512f
#define _2PI 				6.28318530718f
#define _3PI_2 				4.71238898038f
#define _PI_6 				0.52359877559f

// 四舍五入取整
#define _round(x)                       ((x) >= 0 ? (long)((x) + 0.5f) : (long)((x)-0.5f))
// 判断上下限
#define _constrain(amt, low, high)      ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// 判断正负
#define _sign(a)                        (((a) < 0) ? -1 : ((a) > 0))
// 取绝对值
#define _abs(a)                         ((a) < 0 ? -(a) : (a))

#define NOT_SET                         -123
#define _isset(a)                       ((a) != (NOT_SET))

// dq current structure
struct DQCurrent_s
{
    float d;
    float q;
};

// phase current structure
struct PhaseCurrent_s
{
    float a;
    float b;
    float c;
};

// dq voltage structs
struct DQVoltage_s
{
    float d;
    float q;
};

float _sin(float a);                                       // 0-2PI 求sin
float _cos(float a);                                       // 0-2PI 求cos
void _sincos(float a, float* s, float* c);				   // 同时求sin和cos
float _normalizeAngle(float angle);                        // 2PI 取余
float _electricalAngle(float shaft_angle, int pole_pairs); // 计算电角度
float _sqrtApprox(float value);                            // 开方

#endif
